#include <opencv2/opencv.hpp>
#include <iostream>
#include <chrono>
#include <sys/time.h>
#include <unistd.h>

using namespace std;
using namespace cv;

#define MILLION 1000000L

int main(int, char *argv[])
{
    Mat in_image, out_image;
    //读取原始图像
    in_image = imread("../test.png", IMREAD_GRAYSCALE);
    if (in_image.empty()) {
        //检查是否读取图像
        cout << "Error! Input image cannot be read...\n";
        return -1;
    }else{
        cout << "Read image OK :) " << endl;
        cout << "And its properties are list as follows:" << endl;
        cout << "> rows: " << in_image.rows << endl;
        cout << "> cols: " << in_image.cols << endl;
        cout << "> channels/depthts: " << in_image.channels() << endl;
        cout << "> elemSize: " << in_image.elemSize() << endl;
        cout << "> step: " << in_image.step1() << endl;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    /**************************************************************/
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;
    // 去畸变以后的图
    int rows = in_image.rows;
    int cols = in_image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   
    int u_distorted, v_distorted;
    double x, y;
    // 计算去畸变后图像的内容
    //for (int v = 0; v < rows; v++){
    //    for (int u = 0; u < cols; u++) {

    for(int v = rows - 1; v != 0; v--){
        for(int u = cols - 1; u != 0; u--){
            // 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
            // start your code here
            x = (u - cx) / fx;
            y = (v - cy) / fy;
            // double r = sqrt(x*x + y*y);
            // double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
            // double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
            double x2 = x * x;
            double y2 = y * y;
            double xy = x * y;
            double r2 = (x2 + y2);
            double r2_2 = r2*r2;
            double xy_coef = (1 + k1 * r2 + k2 * r2_2);
            double x_distorted = x * xy_coef + 2 * p1 * xy + p2 * (r2 + 2 * x2);
            double y_distorted = y * xy_coef + p1 * (r2 + 2 * y2) + 2 * p2 * xy;
            u_distorted = (int)(fx * x_distorted + cx);
            v_distorted = (int)(fy * y_distorted + cy);
            // end your code here
            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows){
                (&image_undistort.data[v*cols])[u] = (&in_image.data[v_distorted*cols])[u_distorted];
            }else{
                (&image_undistort.data[v*cols])[u] = 255;
            }
            // if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
            //     image_undistort.at<uchar>(v, u) = in_image.at<uchar>((int) v_distorted, (int) u_distorted);
            // } else {
            //     image_undistort.at<uchar>(v, u) = 0;
            //     cout << "haha" << endl;
            // }
        }
    }
    /**************************************************************/
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used_A = chrono::duration_cast<chrono::duration<double>>(t2 - t1)*MILLION;
    cout << "Method cost " << time_used_A.count() << "us" << endl;

    // 画图去畸变前后图像
    cv::imshow("image raw", in_image);
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}
